/*
 * collision_detector.cc
 *
 *  Created on: Apr 7, 2010
 *      Author: Aron
 * takes sensor-queue, sensor number (uses this and it+1 as 2nd) and use same as number as id from sensor_read, minimum distance allowed
 *
 */

#include "xtask.h"
#include "rrcon.h"
#include "log.h"
#include "sensor_read.h"
#include "collision_detector.h"

//sensor placement
#define FRONT       4
#define REAR        5
#define FRONTLEFT   1
#define REARLEFT    3
#define FRONTRIGHT  0
#define REARRIGHT   2

using namespace kthrobot;
using namespace std;

namespace Group5 {

  Collision_Detector::Collision_Detector(SensorsDataQueue *data_queue,int nr)
        : XTask("CollisionDetector"),
          data_queue_(data_queue),
          numberOfSensors_(nr){
          SetPeriodHz(hz_);
          for (int i = 0; i < 6; i++){
            range_[i]= 0;
          }

        }

    Collision_Detector::~Collision_Detector() {
        Join();
    }

  int Collision_Detector::GetRange(int type){
        return range_[type];
  }

  void Collision_Detector::TaskCode() {
    ADCRawData data;

    while(!MustStop()) {


      if (data_queue_->Deq(&data, 0.2)) {
        for (int i = 0; i < numberOfSensors_; i++){
          range_[i]= SensorsReader::ConvertToCm(data.channel[i],i);
        }
      }
      WaitPeriod();
    }
};
};
